#include "auto_navigation/planning/planner_manager.hpp"

namespace auto_navigation {
namespace planning {

PlannerManager::PlannerManager() : nh_(nullptr), map_received_(false), pointcloud_received_(false) {
}

PlannerManager::~PlannerManager() {
    // 清理资源
    planners_.clear();
    current_planner_.reset();
}

bool PlannerManager::initialize(ros::NodeHandle& nh) {
    nh_ = &nh;
    
    // 订阅规划器切换话题
    planner_switch_sub_ = nh.subscribe("/planning/switch_planner", 1, 
                                      &PlannerManager::plannerSwitchCallback, this);
    
    ROS_INFO("Planner Manager initialized");
    return true;
}

bool PlannerManager::registerPlanner(PlannerInterfacePtr planner) {
    if (!planner) {
        ROS_ERROR("Cannot register null planner");
        return false;
    }
    
    std::string name = planner->getName();
    if (planners_.find(name) != planners_.end()) {
        ROS_WARN("Planner with name '%s' already registered, overwriting", name.c_str());
    }
    
    // 初始化规划器
    if (!planner->initialize(*nh_)) {
        ROS_ERROR("Failed to initialize planner '%s'", name.c_str());
        return false;
    }
    
    // 注册规划器
    planners_[name] = planner;
    
    // 如果是第一个注册的规划器，设为当前规划器
    if (!current_planner_) {
        current_planner_ = planner;
        ROS_INFO("Set '%s' as current planner", name.c_str());
    }
    
    // 如果已经接收到地图，设置给新注册的规划器
    if (map_received_) {
        planner->setMap(current_map_);
    }
    
    ROS_INFO("Registered planner '%s'", name.c_str());
    return true;
}

bool PlannerManager::switchPlanner(const std::string& name) {
    auto it = planners_.find(name);
    if (it == planners_.end()) {
        ROS_ERROR("Planner '%s' not found", name.c_str());
        return false;
    }
    
    current_planner_ = it->second;
    ROS_INFO("Switched to planner '%s'", name.c_str());
    return true;
}

void PlannerManager::setMap(const nav_msgs::OccupancyGrid& map) {
    current_map_ = map;
    map_received_ = true;
    
    // 更新所有规划器的地图
    for (auto& planner_pair : planners_) {
        planner_pair.second->setMap(map);
    }
    
    ROS_INFO("Map updated for all planners");
}

bool PlannerManager::plan(const geometry_msgs::PoseStamped& start,
                         const geometry_msgs::PoseStamped& goal,
                         nav_msgs::Path& path) {
    if (!current_planner_) {
        ROS_ERROR("No planner available");
        return false;
    }
    
    // 检查是否有地图数据或点云数据
    if (!map_received_ && !pointcloud_received_) {
        ROS_ERROR("No map received yet");
        return false;
    }
    
    ROS_INFO("Planning with '%s'", current_planner_->getName().c_str());
    return current_planner_->plan(start, goal, path);
}

std::string PlannerManager::getCurrentPlannerName() const {
    return current_planner_ ? current_planner_->getName() : "none";
}

std::vector<std::string> PlannerManager::getAvailablePlanners() const {
    std::vector<std::string> names;
    for (const auto& planner_pair : planners_) {
        names.push_back(planner_pair.first);
    }
    return names;
}

void PlannerManager::plannerSwitchCallback(const std_msgs::String::ConstPtr& msg) {
    std::string planner_name = msg->data;
    if (switchPlanner(planner_name)) {
        ROS_INFO("Switched to planner '%s' via topic", planner_name.c_str());
    } else {
        ROS_ERROR("Failed to switch to planner '%s'", planner_name.c_str());
    }
}

PlannerInterfacePtr PlannerManager::getPlannerByName(const std::string& name) const {
    auto it = planners_.find(name);
    if (it != planners_.end()) {
        return it->second;
    }
    return nullptr;
}

} // namespace planning
} // namespace auto_navigation 